/*  This file is part of Real Time Digital Audio Processing library.
 *  Copyright (C) 2008 - Emilio Monti - emilmont@gmail.com
 *
 *  Real Time Digital Audio Processing library is free software: you can
 *  redistribute it and/or modify it under the terms of the GNU General
 *  Public License as published by the Free Software Foundation, either
 *  version 2 of the License, or (at your option) any later version.
 *
 *  Real Time Digital Audio Processing library is distributed in the hope
 *  that it will be useful, but WITHOUT ANY WARRANTY; without even the
 *  implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 *  See the GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Real Time Audio Processing library.
 *  If not, see <http://www.gnu.org/licenses/>.
 */
#include <stddef.h>
#include <string.h>

#include "Filter.h"

/**
 * Finite Impulse Filter
 *
 * @param order The order of the FIR filter
 * @param coeff The filter coefficients
 *
 * @see FIR::processSignal
 */
FIR::FIR(int order, float* coeff) {
    sprintf(descr, "[#] FIR Filter");

    this->order = order;
    this->coeff = coeff;
    w = new float[order];
    for (int i=0; i<order; i++) {
        w[i] = 0.0;
    }
}

/**
 * FIR signal processing
 * @param signal The signal buffer to be processed
 * @param length The signal buffer length
 *
 * Linear convolution of filter coefficients and the input signal:
 * \f[
 * \ y[n] = \sum_{i=0}^{N} c_i x[n-i]
 * \f]
 */
void FIR::processSignal(float* signal, unsigned long length) {
    for (unsigned long i=0; i<length; i++) {
        // Get current data to delay line
        w[0] = signal[i];

        // FIR filter processing
        signal[i] = 0;
        for (int j=0; j<order; j++)
            signal[i] += coeff[j]*w[j];

        // Update signal buffer
        for (int j=order-1; j>0; j--)
            w[j] = w[j-1];
    }
}

FIR::~FIR() {
    delete [] w;
}

/**
 * Infinite Impulse Filter
 * @param sections The number of second-order sections of the filter
 * @param gain The filter gain
 * @param coeff The filter coefficients
 *
 * Matrix sos containing the coefficients of each second-order section
 * determined from its zero-pole form:
 * \f[
 * \ sos = \left[ \begin{array}{cccccc}
 * b_{00} & b_{01} & b_{02} & 1 & a_{00} & a_{01} \\
 * ... & ... & ... & ... & ... & ... \\
 * b_{k0} & b_{k1} & b_{k2} & 1 & a_{k0} & a_{k1}
 * \end{array} \right]
 * \f]
 *
 * Example usage:
 * \code
 * float coeff[] = {
 *      b_00, b_01, b_02, a_00, a_01,
 *      b_10, b_11, b_12, a_10, a_11
 * };
 *
 * filter = new IIR(2,  0.75, coeff);
 * \endcode
 *
 * @see IIR::processSignal
 */
IIR::IIR(unsigned int sections, float gain, float* coeff) {
    sprintf(descr, "[#] IIR Filter");

    this->sections = sections;
    this->gain = gain;

    c = new float*[sections];
    w = new float*[sections];
    for (unsigned int i=0; i<sections; i++) {
        // For each section there is a group of coefficients:
        // b_0, b_1, b_2, a_0, a_1
        c[i] = new float[5];
        for (int j=0; j<5; j++)
            c[i][j] = coeff[i*5+j];

        // For each section there is one signal buffer:
        // w(n), w(n-1), w(n-2)
        w[i] = new float[3];
        for (int j=0; j<3; j++)
            w[i][j] = 0.0;
    }
}

/**
 * IIR signal processing
 * @param signal The signal buffer to be processed
 * @param length The signal buffer length
 *
 * Direct-Form II Cascade IIR Filter:
 * \f{eqnarray*}
 * \ w[n] = x[n] - a_1 w(n - 1) - a_2 w(n - 2)     \\
 * \ y[n] = b_0 w(n) + b_1 w(n - 1) + b_2 w(n - 2)
 * \f}
 */
void IIR::processSignal(float* signal, unsigned long length) {
    for (unsigned long i=0; i<length; i++) {
        float temp = signal[i];
        for (unsigned int k=0; k<sections; k++) {
            w[k][0] = temp - c[k][3]*w[k][1] - c[k][4]*w[k][2];
            temp    = c[k][0]*w[k][0] + c[k][1]*w[k][1] + c[k][2]*w[k][2];
            w[k][2] = w[k][1]; // w(n-2) <- w(n-1)
            w[k][1] = w[k][0]; // w(n-1) <- w(n)
        }
        signal[i] = gain * temp;
    }
}

IIR::~IIR() {
    for (unsigned int i=0; i<sections; i++) {
        delete [] c[i];
        delete [] w[i];
    }
    delete [] c;
    delete [] w;
    c = NULL;
    w = NULL;
}
